import pyads
import threading
from ctypes import sizeof
import numpy as np
import time
import copy

class pogo_device(object):
    def __init__(self,pyads_addr=None, ip_addr=None, pogo_axis_index=None):
        if pyads_addr is None:
            pyads_addr = "192.168.1.5.1.1"

        if ip_addr is None:
            ip_addr = "192.168.1.5"

        #plc = pyads.Connection('192.168.1.5.1.1',pyads.PORT_SPS1)

        if pogo_axis_index is not None:
            self.plc = pyads.Connection(pyads_addr,pyads.PORT_TC3PLC1,ip_addr)
            self.plc.open()
            self.basicInterface(pogo_axis_index)
            self.lock = threading.Lock()

    def basicInterface(self,pogo_axis_index):
        override=self.plc.read_by_name('ControlPannelManagement.rOverride',pyads.PLCTYPE_REAL)
        print('override=',override)

        # self.adsCallStatusNames = [
        #     ".g_sAxisArr[1].NcToPlc.ActPos",
        #     ".g_sAxisArr[2].NcToPlc.ActPos",
        #     ".g_sAxisArr[3].NcToPlc.ActPos",
        #     ".g_sAxisArr[4].NcToPlc.ActPos",
        #     ".g_sAxisArr[5].NcToPlc.ActPos",
        #     ".g_sAxisArr[6].NcToPlc.ActPos"
        # ]

        self.adsCallStatusNames = []
        for i in pogo_axis_index:
            single_pogo_index = [".g_sAxisArr["+str(j)+"].NcToPlc.ActPos" for j in i]
            self.adsCallStatusNames.extend(single_pogo_index)

        self.pogo_axis_index = pogo_axis_index

    def enableBeckhoffMotor(self,isEnable=True):
        self.lock.acquire()
        self.plc.write_by_name('MC_FIFO.externalEnable',isEnable,pyads.PLCTYPE_BOOL)
        self.lock.release()
        return isEnable

    def readLegControlLength(self):
        self.lock.acquire()
        adsCallStatus=self.plc.read_list_by_name(self.adsCallStatusNames)
        self.lock.release()

        retList = [adsCallStatus[i]/1000.0 for i in self.adsCallStatusNames] 

        ret = []
        id = 0
        for i in range(len(self.pogo_axis_index)):
            single_record = []
            for j in range(len(self.pogo_axis_index[i])):
                single_record.append(retList[id])
                id = id +1
            ret.append(single_record)

        return ret 

    def close(self):
        self.plc.close()

class pogo_device_nofity(pogo_device):
    def __init__(self, pyads_addr = None, ip_addr=None):
        super(beckhoff_device_nofity,self).__init__(pyads_addr,ip_addr)
        self.controlInterface()

    def controlInterface(self):
        self.adsCallParaListNames = [
            'MC_FIFO.stADSSharedMem.bReturnSignal',
            'MC_FIFO.stADSSharedMem.nFuntionCode',
            'MC_FIFO.stADSSharedMem.nReturnCode',
            'MC_FIFO.stADSSharedMem.nInputLen'
            ]
        adsCallParaList=self.plc.read_list_by_name(self.adsCallParaListNames)
        arrInput = self.plc.read_by_name('MC_FIFO.stADSSharedMem.arrInput', pyads.PLCTYPE_LREAL * adsCallParaList['MC_FIFO.stADSSharedMem.nInputLen'])

        self.AdsCallEvent = threading.Event()

        self.adsCallSignal = self.plc.get_symbol("MC_FIFO.stADSSharedMem.bADSCallSignal")
        self.adsCallSignal.auto_update = True
        atr = pyads.NotificationAttrib(sizeof(pyads.PLCTYPE_BOOL))
        self.adsCallSignal.add_device_notification(self.adsCallback,atr)

    def adsCallback(self, notification, data):
        contents = notification.contents
        #value = next(map(bool,bytearray(contents.data)[0:contents.cbSampleSize]))

        value = contents.data
        #print("recv:",contents.data)
        if (value):
            self.AdsCallEvent.set()

    def restartFifo(self):
        self.lock.acquire()
        self.plc.write_by_name('MC_FIFO.stFifoInterface.bStart',True,pyads.PLCTYPE_BOOL)
        self.lock.release()
        time.sleep(0.1)

    def startFifoRunning(self):
        self.lock.acquire()
        self.plc.write_by_name('MC_FIFO.stFifoInterface.bChannelStart',True,pyads.PLCTYPE_BOOL)
        self.plc.write_by_name('MC_FIFO.stFifoInterface.bSetOverride',True,pyads.PLCTYPE_BOOL)
        self.lock.release()

    def enableBeckhoffMotor(self,isEnable=True):
        self.lock.acquire()
        self.plc.write_by_name('MC_FIFO.externalEnable',isEnable,pyads.PLCTYPE_BOOL)
        self.lock.release()
        return isEnable

    def isBeckhoffMotorEnabled(self):
        self.lock.acquire()
        ret = self.plc.read_by_name('MC_FIFO.externalEnable',pyads.PLCTYPE_BOOL)
        self.lock.release()
        return ret

    def disableFifo(self):
        self.lock.acquire()
        self.plc.write_by_name('MC_FIFO.stFifoInterface.bStart',False,pyads.PLCTYPE_BOOL)
        self.plc.write_by_name('MC_FIFO.stFifoInterface.bChannelStart',False,pyads.PLCTYPE_BOOL)
        self.plc.write_by_name('MC_FIFO.stFifoInterface.bSetOverride',False,pyads.PLCTYPE_BOOL)
        self.lock.release()
        return

    def echoTrajectoryProc(self,controllers,wait=0.1):
        # for i in controllers:
        #     memRet,dataLen = i.RunControlStep(6,50)
        #     if(dataLen != 0):
        #         break
        # return dataLen
        directWrite = False
        if not wait > 1e-6:
            directWrite = True

        if(directWrite or (self.AdsCallEvent.wait(wait) and self.AdsCallEvent.is_set())):
            self.AdsCallEvent.clear()

            self.lock.acquire()

            adsCallParaList=self.plc.read_list_by_name(self.adsCallParaListNames)
            #print(adsCallParaList)
            paraNum = adsCallParaList['MC_FIFO.stADSSharedMem.nInputLen']

            if(paraNum>0):
                arrInput = self.plc.read_by_name('MC_FIFO.stADSSharedMem.arrInput', pyads.PLCTYPE_LREAL * adsCallParaList['MC_FIFO.stADSSharedMem.nInputLen'])
                print(arrInput)

            col = int(arrInput[0])
            row = int(arrInput[1])

            for i in controllers:
                memData,dataLen = i.RunControlStep(col,row)
                memRet = [i*1000.0 for i in memData]
                if(dataLen != 0):
                    break

            if(dataLen>0):
                self.plc.write_by_name('MC_FIFO.stADSSharedMem.arrOutput',memRet,pyads.PLCTYPE_LREAL*dataLen)
            adsCallParaList['MC_FIFO.stADSSharedMem.bReturnSignal']=1
            adsCallParaList['MC_FIFO.stADSSharedMem.nReturnCode']=dataLen
            self.plc.write_list_by_name(adsCallParaList)

            self.lock.release()

        else:
            return -1
        return dataLen

if __name__ == '__main__':
    print("starting pogo driver node")

    device = pogo_device("192.168.1.103.1.1","192.168.1.100")
    result = device.readLegControlLength()
    print(result)

    device.close()
